#!/usr/bin/env python
import roslib; roslib.load_manifest('bwi_dispatcher')
import rospy
import bwi_dispatcher
import bwi_dispatcher.srv
import bwi_dispatcher.msg
from bwi_dispatcher.msg import MachineStatus

import gateway_comms.msg
import gateway_comms.srv

class Dispatcher():
  def __init__(self):
    
    self.publishRate = rospy.get_param('~publish_rate', 5)
    self.updateRate = rospy.get_param('~update_rate', 1)
    self.maxDuration = rospy.get_param('~max_status_duration', 2)
    self.gateway = rospy.get_param('~gateway', 'gateway')
    self.node = "bwi_dispatcher"
    self.dservices, self.gservices = self.__setupRosServices()
    self.__setupStatus()

  def __setupStatus(self):
    self.status = MachineStatus()
    self.status.devices = []
    self.status.applications = []
    self.status.machine = rospy.get_param('~machine_name', 'machine1')
    self.statuses = {}
    
    statusTopic = rospy.get_param('~status_topic', '/machine')
    self.publisher = rospy.Publisher(statusTopic, MachineStatus)
    self.subscriber = rospy.Subscriber(statusTopic, MachineStatus, lambda x: self.handleStatus(x))

    request = gateway_comms.srv.RemoteRequest()
    request.remote.gateway = ".*"
    request.remote.rule.name = statusTopic
    request.remote.rule.type = gateway_comms.msg.ConnectionType.PUBLISHER
    self.gservices['pull'](request)
    request = gateway_comms.srv.AdvertiseRequest()
    rule = gateway_comms.msg.Rule()
    rule.name = statusTopic
    rule.type = gateway_comms.msg.ConnectionType.PUBLISHER
    rule.node = self.node
    request.rules += [rule]
    self.gservices['advertise'](request)

  def __setupRosServices(self):
    dispatcher_services = {}
    dispatcher_services['list_remote_publications'] = rospy.Service('~list_remote_publications', bwi_dispatcher.srv.ListRemoteAdvertisements, self.rosServiceListRemotePublications)
    dispatcher_services['list_remote_subscriptions'] = rospy.Service('~list_remote_subscriptions', bwi_dispatcher.srv.ListRemoteAdvertisements, self.rosServiceListRemoteSubscriptions)
    dispatcher_services['list_remote_services'] = rospy.Service('~list_remote_services', bwi_dispatcher.srv.ListRemoteAdvertisements, self.rosServiceListRemoteServices)
    dispatcher_services['advertise_device'] = rospy.Service('~advertise_device', bwi_dispatcher.srv.AdvertiseNode, self.rosServiceAdvertiseDevice)
    dispatcher_services['advertise_application'] = rospy.Service('~advertise_application', bwi_dispatcher.srv.AdvertiseNode, self.rosServiceAdvertiseApplication)
    # ... snip ... #

    gateway_services = {}
    gateway_services['advertise'] = rospy.ServiceProxy('/%s/advertise' % self.gateway, gateway_comms.srv.Advertise)
    gateway_services['advertise_all'] = rospy.ServiceProxy('/%s/advertise_all' % self.gateway, gateway_comms.srv.AdvertiseAll)
    gateway_services['remote_gateway_info'] = rospy.ServiceProxy('/%s/remote_gateway_info' % self.gateway, gateway_comms.srv.RemoteGatewayInfo)
    gateway_services['pull'] = rospy.ServiceProxy('/%s/pull' % self.gateway, gateway_comms.srv.Remote)
  
    return dispatcher_services, gateway_services

  def __publishStatus(self):
    self.status.timestamp = rospy.Time.now()
    self.publisher.publish(self.status)
    rospy.loginfo("publishing status for %s" % self.status.machine)

  def handleStatus(self, status):
    self.statuses[status.machine] = status
    rospy.loginfo("handling status")

  def updateStatuses(self):
    for machine in self.statuses.keys():
      s = self.statuses[machine]
      duration = rospy.Time.now() - s.timestamp
      if duration.secs > self.maxDuration:
        del self.statuses[machine]
    rospy.loginfo("updating status")

  def spin(self):
    rospy.Timer(rospy.Duration(self.publishRate), lambda x: self.__publishStatus())
    rospy.Timer(rospy.Duration(self.updateRate), lambda x: self.updateStatuses())
    rospy.spin()    

  ################################################################
  # Ros Service Callbacks
  ###############################################################

  def rosServiceListRemoteAdvertisements(self, msg, type):
    response = bwi_dispatcher.srv.ListRemoteAdvertisementsResponse()
    request = gateway_comms.srv.RemoteGatewayInfoRequest()
    if msg.gateways:
      request.gateways = msg.gateways
    r = self.gservices['remote_gateway_info'](request)
    for gateway in r.gateways:
      for rule in gateway.public_interface:
        if rule.type == type:
          response.advertisements += [rule.name]
    return response

  def rosServiceListRemotePublications(self, msg):
    return self.rosServiceListRemoteAdvertisements(msg, gateway_comms.msg.ConnectionType.PUBLISHER)

  def rosServiceListRemoteSubscriptions(self, msg):
    return self.rosServiceListRemoteAdvertisements(msg, gateway_comms.msg.ConnectionType.SUBSCRIBER)

  def rosServiceListRemoteServices(self, msg):
    return self.rosServiceListRemoteAdvertisements(msg, gateway_comms.msg.ConnectionType.SERVICE)

  def rosServiceListDevices(self, msg):
    response = bwi_dispatcher.srv.ListDevicesResponse()
    response.devices = self.status.devices
    return response

  def rosServiceListApplications(self, msg):
    response = bwi_dispatcher.srv.ListApplicationsResponse()
    response.applications = self.status.applications
    return response

  def rosServiceListMachines(self, msg):
    response = bwi_dispatcher.srv.ListMachinesResponse()
    response.machines = self.statuses.keys()
    return response

  def rosServicePullAll(self, pull):
    response = bwi_dispatcher.srv.PullAllResponse()
    request = gateway_comms.srv.RemoteAllRequest()
    request.cancel = pull.cancel
    request.gateway = pull.machine
    self.gservices['pull_all'](request)
    return response

  def rosServicePullTopic(self, pull):
    response = bwi_dispatcher.srv.PullTopicResponse()
    request = gateway_comms.srv.RemoteRequest()
    request.cancel = pull.cancel
    r = gateway_comms.msg.RemoteRule()
    r.gateway = pull.machine
    r.rule.type = pull.type
    r.rule.name = pull.name
    r.rule.node = pull.node
    request.remote = r
    self.gservices['pull'](request)
    return response

  def rosServiceAdvertiseAll(self, msg):
    response = bwi_dispatcher.srv.AdvertiseAllResponse()
    request = gateway_comms.srv.AdvertiseAllRequest()
    request.cancel = msg.cancel
    self.gservices['advertise_all'](request)
    return response

  def rosServiceAdvertiseNode(self, msg):
    response = bwi_dispatcher.srv.AdvertiseNodeResponse()
    request = gateway_comms.srv.AdvertiseRequest()
    request.cancel = msg.cancel
    request.rules = []
    for ad in msg.advertisements:
      r = gateway_comms.msg.Rule()
      r.type = ad.type
      r.name = ad.name
      r.node = msg.node_name
      request.rules += [r]
    r = self.gservices['advertise'](request)
    if r.result == gateway_comms.msg.Result.SUCCESS:
      response.success = True
    else:
      rospy.loginfo("Gateway response: %d" % r.result)
    return response

  def rosServiceAdvertiseApplication(self, msg):
    msg.node_type = bwi_dispatcher.msg.NodeType.APPLICATION
    filter(lambda a: a.name != msg.node_name, self.status.applications)
    response = self.rosServiceAdvertiseNode(msg)
    if not msg.cancel and response.success:
      a = self.getNodeStatus(msg, bwi_dispatcher.msg.Application())
      self.status.applications += [a]
    return response

  def rosServiceAdvertiseDevice(self, msg):
    msg.node_type = bwi_dispatcher.msg.NodeType.DEVICE
    filter(lambda d: d.name != msg.node_name, self.status.devices)
    response = self.rosServiceAdvertiseNode(msg)
    if not msg.cancel and response.success:
      d = self.getNodeStatus(msg, bwi_dispatcher.msg.Device())
      self.status.applications += [d]
    return response

  def getNodeStatus(self, msg, status):
    status.topics = []
    for ad in msg.advertisements:
      if ad.type == bwi_dispatcher.msg.AdvertisementType.PUBLISHER:
        t = bwi_dispatcher.msg.Topic()
        t.name = ad.name
        status.topics += [t]
      if ad.type == bwi_dispatcher.msg.AdvertisementType.SERVICE:
        s = bwi_dispatcher.msg.Service()
        s.name = ad.name
        status.services += [s]
    status.name = msg.node_name
    return status

  def clearRules(self):
    rospy.loginfo("Clearing all rules")
    request = gateway_comms.srv.AdvertiseAllRequest()
    request.cancel = True
    response = self.gservices['advertise_all'](request)
    if response.result != gateway_comms.msg.Result.SUCCESS:
      rospy.loginfo("Gateway response: %d" % r.result)

  def shutdown(self):
    self.clearRules()

if __name__ == '__main__':
  rospy.init_node('dispatcher')
  dispatcher = Dispatcher()
  rospy.on_shutdown(dispatcher.shutdown)
  dispatcher.spin()
  rospy.loginfo('Dispatcher shutting down.')
